//#include <termios.h>
//#include <signal.h>
//#include <math.h>
//#include <stdio.h>
//#include <stdlib.h>
//#include <sys/poll.h>
//
//#include <boost/thread/thread.hpp>
//#include <ros/ros.h>
//#include <geometry_msgs/TwistStamped.h>
//#include <geometry_msgs/PoseStamped.h>
//#include <std_msgs/builtin_bool.h>
//#include <fstream>
//#include <iostream>
//#include <string>
//
//
//#include <Eigen/Core>
//#include <Eigen/SVD>
//#include <Eigen/Cholesky>
//#include <Eigen/Geometry>
//#include <Eigen/LU>
//
//#include "eigen_conversions/eigen_msg.h"
//#include <mavros_msgs/CommandBool.h>
//#include <mavros_msgs/CommandTOL.h>
//#include <mavros_msgs/ParamGet.h>
//#include <mavros_msgs/ParamSet.h>
//#include <geometry_msgs/Vector3Stamped.h>
//#include <mavros_msgs/SetMode.h>
//#include <mavros_msgs/State.h>
//using namespace Eigen;
//using Eigen::Matrix3d;
//using Eigen::Affine3d;
//using Eigen::Vector3d;
//using Eigen::Matrix3d;
//using std::string;
//using namespace std;
//
//class TestOpt
//{
//    private:
//        geometry_msgs::PoseStamped pose_sp_keyboard;// from keyboad commmand
//        geometry_msgs::TwistStamped vel_modify_keyboard;// from keyboad commmand
//        ros::NodeHandle nh;
//        ros::Publisher pose_set_pub;
//        ros::Publisher vel_modify_set_pub;
//
//        ros::Subscriber uavpose_sub;
//        ros::Subscriber key_mission;
//        geometry_msgs::PoseStamped pose_init;
//
//        ros::Subscriber mocap_vrpn_sub_1;
//        ros::Subscriber mocap_vrpn_STANDERD_sub;
//
//        string test_num;
//        string STANDERD_num;
//
//        //
//        ros::Subscriber mavros_pose_sub;
//        ros::Publisher position_pub;
//
//        bool StateIsInit;
//        bool StartOffControl;
//
//
//    	Affine3d pose_opti_enu;
//    	Affine3d pose_uav_rb;
//
//    	Affine3d opti_pose;
//    	Affine3d opti_pose_standerd;
//
//
//    	string file_name1;
//    	string file_name2;
//
//    	ofstream path_ofs_1;
//    	ofstream path_ofs_2;
//
//
//    	//
//    	string file_send_pose;
//    	string file_receive_pose;
//    	ofstream path_ofs_send;
//    	ofstream path_ofs_receive;
//
//    	//
//    	Affine3d mavros_pose;
//    	Affine3d opti_pose_test_relay;
//
//    	double  timebias;
//    public:
//        void optitrack_pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg);
//        void optitrack_pose_standerd_callback(const geometry_msgs::PoseStamped::ConstPtr& msg);
//
//        void mavros_pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg);
//        void send_uav_pose();
//
//        TestOpt()
//        {
//        	timebias = ros::Time::now().toSec();
//
//        	file_name1 = "/home/throne/test1.txt";
//        	file_name2 = "/home/throne/test2.txt";
//
//        	file_send_pose = "/home/throne/test_send.txt";
//        	file_receive_pose = "/home/throne/test_receive.txt";
//
//        	path_ofs_1.clear();
//        	path_ofs_1.open(file_name1.c_str(),ios::in | ios::out | ios::binary |ios::app);
//        	path_ofs_2.clear();
//        	path_ofs_2.open(file_name2.c_str(),ios::in | ios::out | ios::binary | ios::app);
//
//
//         	path_ofs_send.clear();
//         	path_ofs_send.open(file_send_pose.c_str(),ios::in | ios::out | ios::binary |ios::app);
//         	path_ofs_receive.clear();
//         	path_ofs_receive.open(file_receive_pose.c_str(),ios::in | ios::out | ios::binary | ios::app);
//
//        	test_num = '9';
//        	STANDERD_num = '3';
//
//        	//test 1
//        	mocap_vrpn_sub_1 = nh.subscribe(string("/vrpn_client_node/") + string("MiniQuad_") + test_num + string("/pose"), 1, &TestOpt::optitrack_pose_callback, this);
//        	mocap_vrpn_STANDERD_sub = nh.subscribe(string("/vrpn_client_node/") + string("MiniQuad_") + STANDERD_num + string("/pose"), 1, &TestOpt::optitrack_pose_standerd_callback, this);
//
//        	//test 2
//        	//test transform time
////        	mavros_pose_sub = nh.subscribe(string("MiniQuad_") + test_num  + string("/mavros/local_position/pose"), 1, &TestOpt::mavros_pose_callback, this);
////        	position_pub = nh.advertise<geometry_msgs::PoseStamped>(string("MiniQuad_") + test_num + string("/mavros/mocap/pose"), 1);
//
//
//        	pose_opti_enu = Affine3d::Identity();
//        	pose_opti_enu.linear()<<1,0,0,0,0,-1,0,1,0;
//
//        	Matrix3d pose_init_uav_enu;
//        	pose_init_uav_enu<<0,-1,0,1,0,0,0,0,1;
//
//        	pose_uav_rb.linear() = pose_opti_enu.linear().inverse() * pose_init_uav_enu;
//        	pose_uav_rb.translation() = Vector3d::Zero();
//        }
//
//        ~TestOpt(){}
//        void keyboardLoop();
//        void getkey_cb(const boost::shared_ptr<bool const>& msg){}
//        void getPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){}
//        void stopRobot()
//        {
//        	StartOffControl = false;
//        	StateIsInit = false;
//        	path_ofs_1.close();
//        	path_ofs_2.close();
//        	path_ofs_send.close();
//        	path_ofs_receive.close();
//        }
//};
//
//Vector3d to_euler2(Eigen::Matrix<double,3,3> data){
//	Vector3d euler;
//	euler(1) = asinf(- (float)data(2,0));
//
//	if (fabsf((float)euler(1) -1.5708f) < 1.0e-3f) {
//		euler(0) = 0.0f;
//		euler(2) = atan2f((float)data(1,2) - (float)data(0,1), (float)data(0,2) + (float)data(1,1)) + euler(0);
//
//	} else if (fabsf((float)euler(1) + 1.5708f) < 1.0e-3f) {
//		euler(0) = 0.0f;
//		euler(2) =atan2f((float)data(1,2) - (float)data(0,1), (float)data(0,2) + (float)data(1,1)) - euler(0);
//
//	} else {
//		euler(0) = atan2f((float)data(2,1), (float)data(2,2));
//		euler(2) = atan2f((float)data(1,0), (float)data(0,0));
//	}
//
//	return euler;
//}
//
//void TestOpt::send_uav_pose()
//{
//	ROS_INFO("Send_OK");
//		static int count = 0;
//		static int count2 = 0;
//
//		static int num2 = 0;
//		num2++;
//		 if (num2%10 == 0)
//		 {
//			 count += 1;
//		 }
//		 if (num2%30 == 0)
//		{
//					 count2 += 1;
//		}
//		opti_pose_test_relay.translation().x()  = count % 500;
//		opti_pose_test_relay.translation().y()  = count2 ;
//
//		geometry_msgs::PoseStamped pose_msg;
//		pose_msg.header.stamp = ros::Time::now();
//		double time_now = ros::Time::now().toSec() - timebias;
//
//		tf::poseEigenToMsg(opti_pose_test_relay,pose_msg.pose);
//		position_pub.publish(pose_msg);
//
//		//
//		path_ofs_send<<pose_msg.pose.position.x<<'\t' <<pose_msg.pose.position.y <<'\t'<<pose_msg.pose.position.z<<'\t'<<time_now<<endl;
//}
//void TestOpt::mavros_pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
//{
//	tf::poseMsgToEigen(msg->pose,mavros_pose);
//	ROS_INFO("Receive_OK");
//
//	 double time_now = ros::Time::now().toSec() - timebias;
//	//
//	path_ofs_receive<<msg->pose.position.x<<'\t' <<msg->pose.position.y <<'\t'<<msg->pose.position.z<<'\t'<<time_now<<endl;
//}
////
//void TestOpt::optitrack_pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
//{
//	//last_optitrack_timestamp = ros::Time::now().toSec();
//	Affine3d pose_rb_opti;
//	tf::poseMsgToEigen(msg->pose,pose_rb_opti);
//
//
////	Vector3d euler = pose_rb_opti.rotation().eulerAngles(2,1,0);
////	printf("%.4lf %.4lf %.4lf \n", euler.x(), euler.y(), euler.z());
//
//	opti_pose = pose_opti_enu * pose_rb_opti * pose_uav_rb;
//
//	Vector3d euler2;
//	euler2 = to_euler2(opti_pose.linear());
//
//	Vector3d xyz;
//	xyz.x() = opti_pose.translation().x();
//	xyz.y() = opti_pose.translation().y();
//	xyz.z() = opti_pose.translation().z();
//
//
//	 double time_now = ros::Time::now().toSec() - timebias;
//
//
//	path_ofs_1<< xyz.x()<<'\t' <<xyz.y() <<'\t'<<xyz.z()<<'\t'<<euler2.x()<<'\t'<<euler2.y()<<'\t'<<euler2.z()<<'\t'<<time_now<<'\n';
//}
//void TestOpt::optitrack_pose_standerd_callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
//{
//	//last_optitrack_timestamp = ros::Time::now().toSec();
//	Affine3d pose_rb_opti;
//	tf::poseMsgToEigen(msg->pose,pose_rb_opti);
//
//
////	Vector3d euler = pose_rb_opti.rotation().eulerAngles(2,1,0);
////	printf("%.4lf %.4lf %.4lf \n", euler.x(), euler.y(), euler.z());
//
//	opti_pose_standerd = pose_opti_enu * pose_rb_opti * pose_uav_rb;
//	//save
//	Vector3d euler2;
//	euler2 = to_euler2(opti_pose_standerd.linear());
//
//	Vector3d xyz;
//	xyz.x() = opti_pose_standerd.translation().x();
//	xyz.y() = opti_pose_standerd.translation().y();
//	xyz.z() = opti_pose_standerd.translation().z();
//
//	 double time_now = ros::Time::now().toSec() - timebias;
//
//	path_ofs_2<< xyz.x()<<'\t' <<xyz.y() <<'\t'<<xyz.z()<<'\t'<<euler2.x()<<'\t'<<euler2.y()<<'\t'<<euler2.z()<<'\t'<<time_now<<'\n';
//
//
//}
//
////
//TestOpt* tbk;
//int kfd = 0;
////struct termios cooked, raw;
//bool done;
//ofstream path("/home/throne/test_err.txt",ios::in | ios::out | ios::binary |ios::app);
//
//int main(int argc, char** argv)
//{
//    ros::init(argc,argv,"test_opt", ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);
//    TestOpt tbk;
//
// //   boost::thread t = boost::thread(boost::bind(&TestOpt::keyboardLoop, &tbk));
//    const double MAIN_CYCLE_RATE = 120.0;
//    const double MAIN_CYCLE_INTERVAL = 1.0 / MAIN_CYCLE_RATE;
//    ros::Rate main_rate(MAIN_CYCLE_RATE);
//
//    while(ros::ok())
//    {
//    ros::spinOnce();
//    main_rate.sleep();
//    tbk.keyboardLoop();
//    fflush(stdin);
//    }
//    ros::spin();
//    tbk.stopRobot();
//
//  //  tcsetattr(kfd, TCSANOW, &cooked);
//    path.close();
//    return(0);
//}
//
//
//void TestOpt::keyboardLoop()
//{
//
//    //tcsetattr(kfd, TCSANOW, &raw);
//    static int co = 0;
//    {
//    	co++;
//    	double time_n =  ros::Time::now().toSec() - timebias;
//
//    	Vector3d errdis;
//    	Vector3d errangle;
//
//    	errdis = opti_pose_standerd.translation() - opti_pose.translation();
//    	errangle = to_euler2(opti_pose_standerd.linear()) - to_euler2(opti_pose.linear());
//
////    	if(co%5000)
////    	{
//
//    	//test1
//    	if(co>20)
//    	{
//    	path<< co<<'\t'<<errdis.x()<<'\t' <<errdis.y() <<'\t'<<errdis.z()<<'\t'<<errangle.x()<<'\t'<<errangle.y()<<'\t'<<errangle.z()<<'\t'<<time_n<<'\n';
//    	}
////
////    	//test2
////    	send_uav_pose();
//
//    }
//
//}




//test2
////

//
//#include <termios.h>
//#include <signal.h>
//#include <math.h>
//#include <stdio.h>
//#include <stdlib.h>
//#include <sys/poll.h>
//
//#include <boost/thread/thread.hpp>
//#include <ros/ros.h>
//#include <geometry_msgs/TwistStamped.h>
//#include <geometry_msgs/PoseStamped.h>
//#include <std_msgs/builtin_bool.h>
//#include <fstream>
//#include <iostream>
//#include <string>
//
//
//#include <Eigen/Core>
//#include <Eigen/SVD>
//#include <Eigen/Cholesky>
//#include <Eigen/Geometry>
//#include <Eigen/LU>
//
//#include "eigen_conversions/eigen_msg.h"
//#include <mavros_msgs/CommandBool.h>
//#include <mavros_msgs/CommandTOL.h>
//#include <mavros_msgs/ParamGet.h>
//#include <mavros_msgs/ParamSet.h>
//#include <geometry_msgs/Vector3Stamped.h>
//#include <mavros_msgs/SetMode.h>
//#include <mavros_msgs/State.h>
//using namespace Eigen;
//using Eigen::Matrix3d;
//using Eigen::Affine3d;
//using Eigen::Vector3d;
//using Eigen::Matrix3d;
//using std::string;
//using namespace std;
//
//class TestOpt
//{
//    private:
//        geometry_msgs::PoseStamped pose_sp_keyboard;// from keyboad commmand
//        geometry_msgs::TwistStamped vel_modify_keyboard;// from keyboad commmand
//        ros::NodeHandle nh;
//        ros::Publisher pose_set_pub;
//        ros::Publisher vel_modify_set_pub;
//
//        ros::Subscriber uavpose_sub;
//        ros::Subscriber key_mission;
//        geometry_msgs::PoseStamped pose_init;
//
//        ros::Subscriber mocap_vrpn_sub_1;
//        ros::Subscriber mocap_vrpn_STANDERD_sub;
//
//        string test_num;
//        string STANDERD_num;
//
//        //
//        ros::Subscriber mavros_pose_sub;
//        ros::Publisher position_pub;
//
//        bool StateIsInit;
//        bool StartOffControl;
//
//
//    	Affine3d pose_opti_enu;
//    	Affine3d pose_uav_rb;
//
//    	Affine3d opti_pose;
//    	Affine3d opti_pose_standerd;
//
//
//    	string file_name1;
//    	string file_name2;
//
//    	ofstream path_ofs_1;
//    	ofstream path_ofs_2;
//
//
//    	//
//    	string file_send_pose;
//    	string file_receive_pose;
//    	ofstream path_ofs_send;
//    	ofstream path_ofs_receive;
//
//    	//
//    	Affine3d mavros_pose;
//    	Affine3d opti_pose_test_relay;
//
//    	double  timebias;
//    public:
//        void optitrack_pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg);
//        void optitrack_pose_standerd_callback(const geometry_msgs::PoseStamped::ConstPtr& msg);
//
//        void mavros_pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg);
//        void send_uav_pose();
//
//        TestOpt()
//        {
//        	timebias = ros::Time::now().toSec();
//
//        	file_name1 = "/home/throne/test1.txt";
//        	file_name2 = "/home/throne/test2.txt";
//
//        	file_send_pose = "/home/throne/test_send.txt";
//        	file_receive_pose = "/home/throne/test_receive.txt";
//
////        	path_ofs_1.clear();
////        	path_ofs_1.open(file_name1.c_str(),ios::in | ios::out | ios::binary |ios::app);
////        	path_ofs_2.clear();
////        	path_ofs_2.open(file_name2.c_str(),ios::in | ios::out | ios::binary | ios::app);
//
//
//         	path_ofs_send.clear();
//         	path_ofs_send.open(file_send_pose.c_str(),ios::in | ios::out | ios::binary |ios::app);
//         	path_ofs_receive.clear();
//         	path_ofs_receive.open(file_receive_pose.c_str(),ios::in | ios::out | ios::binary | ios::app);
//
//        	test_num = '9';
//        	STANDERD_num = '3';
//
//        	//test 1
//   //     	mocap_vrpn_sub_1 = nh.subscribe(string("/vrpn_client_node/") + string("MiniQuad_") + test_num + string("/pose"), 1, &TestOpt::optitrack_pose_callback, this);
//   //     	mocap_vrpn_STANDERD_sub = nh.subscribe(string("/vrpn_client_node/") + string("MiniQuad_") + STANDERD_num + string("/pose"), 1, &TestOpt::optitrack_pose_standerd_callback, this);
//
//        	//test 2
//        	//test transform time
//        	mavros_pose_sub = nh.subscribe(string("MiniQuad_") + test_num  + string("/mavros/local_position/pose"), 1, &TestOpt::mavros_pose_callback, this);
//       	position_pub = nh.advertise<geometry_msgs::PoseStamped>(string("MiniQuad_") + test_num + string("/mavros/mocap/pose"), 1);
//
//
//        	pose_opti_enu = Affine3d::Identity();
//        	pose_opti_enu.linear()<<1,0,0,0,0,-1,0,1,0;
//
//        	Matrix3d pose_init_uav_enu;
//        	pose_init_uav_enu<<0,-1,0,1,0,0,0,0,1;
//
//        	pose_uav_rb.linear() = pose_opti_enu.linear().inverse() * pose_init_uav_enu;
//        	pose_uav_rb.translation() = Vector3d::Zero();
//        }
//
//        ~TestOpt(){}
//        void keyboardLoop();
//        void getkey_cb(const boost::shared_ptr<bool const>& msg){}
//        void getPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){}
//        void stopRobot()
//        {
//        	StartOffControl = false;
//        	StateIsInit = false;
//        	path_ofs_1.close();
//        	path_ofs_2.close();
//        	path_ofs_send.close();
//        	path_ofs_receive.close();
//        }
//};
//
//Vector3d to_euler2(Eigen::Matrix<double,3,3> data){
//	Vector3d euler;
//	euler(1) = asinf(- (float)data(2,0));
//
//	if (fabsf((float)euler(1) -1.5708f) < 1.0e-3f) {
//		euler(0) = 0.0f;
//		euler(2) = atan2f((float)data(1,2) - (float)data(0,1), (float)data(0,2) + (float)data(1,1)) + euler(0);
//
//	} else if (fabsf((float)euler(1) + 1.5708f) < 1.0e-3f) {
//		euler(0) = 0.0f;
//		euler(2) =atan2f((float)data(1,2) - (float)data(0,1), (float)data(0,2) + (float)data(1,1)) - euler(0);
//
//	} else {
//		euler(0) = atan2f((float)data(2,1), (float)data(2,2));
//		euler(2) = atan2f((float)data(1,0), (float)data(0,0));
//	}
//
//	return euler;
//}
//
//void TestOpt::send_uav_pose()
//{
//	ROS_INFO("Send_OK");
//		static int count = 0;
//		static int count2 = 0;
//
//		static int num2 = 0;
//		num2++;
//		 if (num2%10 == 0)
//		 {
//			 count += 1;
//		 }
//		 if (num2%30 == 0)
//		{
//			 count2 += 1;
//		}
//		opti_pose_test_relay.translation().x()  = count % 500;
//		opti_pose_test_relay.translation().y()  = count2 ;
//
//		geometry_msgs::PoseStamped pose_msg;
//		pose_msg.header.stamp = ros::Time::now();
//		double time_now = ros::Time::now().toSec() - timebias;
//
//		tf::poseEigenToMsg(opti_pose_test_relay,pose_msg.pose);
//		position_pub.publish(pose_msg);
//
//		//
//		path_ofs_send<<pose_msg.pose.position.x<<'\t' <<pose_msg.pose.position.y <<'\t'<<pose_msg.pose.position.z<<'\t'<<time_now<<endl;
//}
//void TestOpt::mavros_pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
//{
//	tf::poseMsgToEigen(msg->pose,mavros_pose);
//	ROS_INFO("Receive_OK");
//
//	 double time_now = ros::Time::now().toSec() - timebias;
//	//
//	path_ofs_receive<<msg->pose.position.x<<'\t' <<msg->pose.position.y <<'\t'<<msg->pose.position.z<<'\t'<<time_now<<endl;
//}
////
//void TestOpt::optitrack_pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
//{
//	//last_optitrack_timestamp = ros::Time::now().toSec();
//	Affine3d pose_rb_opti;
//	tf::poseMsgToEigen(msg->pose,pose_rb_opti);
//
//
//	opti_pose = pose_opti_enu * pose_rb_opti * pose_uav_rb;
//
//	Vector3d euler2;
//	euler2 = to_euler2(opti_pose.linear());
//
//	Vector3d xyz;
//	xyz.x() = opti_pose.translation().x();
//	xyz.y() = opti_pose.translation().y();
//	xyz.z() = opti_pose.translation().z();
//
//
//	 double time_now = ros::Time::now().toSec() - timebias;
//
//
//	//path_ofs_1<< xyz.x()<<'\t' <<xyz.y() <<'\t'<<xyz.z()<<'\t'<<euler2.x()<<'\t'<<euler2.y()<<'\t'<<euler2.z()<<'\t'<<time_now<<'\n';
//}
//void TestOpt::optitrack_pose_standerd_callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
//{
//	//last_optitrack_timestamp = ros::Time::now().toSec();
//	Affine3d pose_rb_opti;
//	tf::poseMsgToEigen(msg->pose,pose_rb_opti);
//
//
//
//	opti_pose_standerd = pose_opti_enu * pose_rb_opti * pose_uav_rb;
//	//save
//	Vector3d euler2;
//	euler2 = to_euler2(opti_pose_standerd.linear());
//
//	Vector3d xyz;
//	xyz.x() = opti_pose_standerd.translation().x();
//	xyz.y() = opti_pose_standerd.translation().y();
//	xyz.z() = opti_pose_standerd.translation().z();
//
//	 double time_now = ros::Time::now().toSec() - timebias;
//
////	path_ofs_2<< xyz.x()<<'\t' <<xyz.y() <<'\t'<<xyz.z()<<'\t'<<euler2.x()<<'\t'<<euler2.y()<<'\t'<<euler2.z()<<'\t'<<time_now<<'\n';
//
//
//}
//
////
//TestOpt* tbk;
//int kfd = 0;
//
////ofstream path("/home/throne/test_err.txt",ios::in | ios::out | ios::binary |ios::app);
//
//int main(int argc, char** argv)
//{
//    ros::init(argc,argv,"test_opt", ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);
//    TestOpt tbk;
//
//
//    double MAIN_CYCLE_RATE = 120.0;
//    double MAIN_CYCLE_INTERVAL = 1.0 / MAIN_CYCLE_RATE;
//    ros::Rate main_rate(MAIN_CYCLE_RATE);
//
//    while(ros::ok())
//    {
//    ros::spinOnce();
//    main_rate.sleep();
//    tbk.keyboardLoop();
//    fflush(stdin);
//    }
//    ros::spin();
//    tbk.stopRobot();
//
//  //  tcsetattr(kfd, TCSANOW, &cooked);
//    path.close();
//    return(0);
//}
//
//
//void TestOpt::keyboardLoop()
//{
//
//    //tcsetattr(kfd, TCSANOW, &raw);
//    static int co = 0;
//    {
//    	co++;
//    	double time_n =  ros::Time::now().toSec() - timebias;
//
//    	Vector3d errdis;
//    	Vector3d errangle;
//
//    	errdis = opti_pose_standerd.translation() - opti_pose.translation();
//    	errangle = to_euler2(opti_pose_standerd.linear()) - to_euler2(opti_pose.linear());
//
////    	if(co%5000)
////    	{
//
//    	//test1
////    	if(co>20)
////    	{
////    	path<< co<<'\t'<<errdis.x()<<'\t' <<errdis.y() <<'\t'<<errdis.z()<<'\t'<<errangle.x()<<'\t'<<errangle.y()<<'\t'<<errangle.z()<<'\t'<<time_n<<'\n';
////    	}
////
//    	//test2
//    	send_uav_pose();
//
//    }
//
//}


//test3

#include <termios.h>
#include <signal.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/poll.h>

#include <boost/thread/thread.hpp>
#include <ros/ros.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/builtin_bool.h>
#include <fstream>
#include <iostream>
#include <string>


#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Cholesky>
#include <Eigen/Geometry>
#include <Eigen/LU>

#include "eigen_conversions/eigen_msg.h"
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/ParamGet.h>
#include <mavros_msgs/ParamSet.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
using namespace Eigen;
using Eigen::Matrix3d;
using Eigen::Affine3d;
using Eigen::Vector3d;
using Eigen::Matrix3d;
using std::string;
using namespace std;
const int MAX_NUM = 5;
class TestOpt
{
    private:
        geometry_msgs::PoseStamped pose_sp_keyboard;// from keyboad commmand
        geometry_msgs::TwistStamped vel_modify_keyboard;// from keyboad commmand
        ros::NodeHandle nh;
        ros::Publisher pose_set_pub;
        ros::Publisher vel_modify_set_pub;

        ros::Subscriber uavpose_sub;
        ros::Subscriber key_mission;
        geometry_msgs::PoseStamped pose_init;

        ros::Subscriber mocap_vrpn_sub_1;
        ros::Subscriber mocap_vrpn_STANDERD_sub;

        string test_num;
        string STANDERD_num;

        //
        ros::Subscriber mavros_pose_sub[MAX_NUM];
        ros::Publisher position_pub[MAX_NUM];

        bool StateIsInit;
        bool StartOffControl;


    	Affine3d pose_opti_enu;
    	Affine3d pose_uav_rb;

    	Affine3d opti_pose;
    	Affine3d opti_pose_standerd;


    	string file_name1;
    	string file_name2;

    	ofstream path_ofs_1;
    	ofstream path_ofs_2;


    	//
    	string file_send_pose;
    	string file_receive_pose;
    	ofstream path_ofs_send[MAX_NUM];
    	ofstream path_ofs_receive[MAX_NUM];
    	int uav_num_now;
    	//
    	Affine3d mavros_pose[MAX_NUM];
    	Affine3d opti_pose_test_relay;

    	//3.1
    	//string file_send_pose;
    	//string file_receive_pose;
       // ofstream path_ofs_send;
       // ofstream path_ofs_receive;

    	double  timebias;
    public:
        void optitrack_pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg);
        void optitrack_pose_standerd_callback(const geometry_msgs::PoseStamped::ConstPtr& msg);

        void mavros_pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg);

        void (*opt_send[MAX_NUM])();
        void (*opt_receive[MAX_NUM])(const geometry_msgs::PoseStamped::ConstPtr& msg);
        void send_uav_pose(int uav_num);
        void send_uav_pose2();
        void send_uav_pose3();
        void mavros_pose_callback2(const geometry_msgs::PoseStamped::ConstPtr& msg);
        void mavros_pose_callback3(const geometry_msgs::PoseStamped::ConstPtr& msg);


        TestOpt(int uav_num = 3)
        {
        	uav_num_now = uav_num;
        	timebias = ros::Time::now().toSec();

        //	file_name1 = "/home/throne/test1.txt";
        //	file_name2 = "/home/throne/test2.txt";

        //	file_send_pose = "/home/throne/test_1_send.txt";
        //	file_receive_pose = "/home/throne/test_1_receive.txt";

//        	path_ofs_1.clear();
//        	path_ofs_1.open(file_name1.c_str(),ios::in | ios::out | ios::binary |ios::app);
//        	path_ofs_2.clear();
//        	path_ofs_2.open(file_name2.c_str(),ios::in | ios::out | ios::binary | ios::app);


          	test_num = '9';
          	STANDERD_num = '3';

//          	opt_send[0] = send_uav_pose;
//          	opt_send[1] = send_uav_pose2;
//          	opt_send[2] = send_uav_pose3;
//          	opt_receive[0] = mavros_pose_callback;
//          	opt_receive[1] = mavros_pose_callback2;
//          	opt_receive[2] = mavros_pose_callback3;


        	mavros_pose_sub[0] = nh.subscribe(string("MiniQuad_") + (char('0' +0))  + string("/mavros/local_position/pose"), 1, &TestOpt::mavros_pose_callback, this);
            position_pub[0] = nh.advertise<geometry_msgs::PoseStamped>(string("MiniQuad_") + (char('0' +0)) + string("/mavros/mocap/pose"), 1);


        	mavros_pose_sub[1] = nh.subscribe(string("MiniQuad_") + (char('0' +1))  + string("/mavros/local_position/pose"), 1, &TestOpt::mavros_pose_callback2, this);
            position_pub[1] = nh.advertise<geometry_msgs::PoseStamped>(string("MiniQuad_") + (char('0' +1)) + string("/mavros/mocap/pose"), 1);

           mavros_pose_sub[2] = nh.subscribe(string("MiniQuad_") + (char('0' +2))  + string("/mavros/local_position/pose"), 1, &TestOpt::mavros_pose_callback3, this);
           position_pub[2] = nh.advertise<geometry_msgs::PoseStamped>(string("MiniQuad_") + (char('0' +2)) + string("/mavros/mocap/pose"), 1);


           for (int i= 0;i<uav_num_now;i++)
        	{
        	string file_send = string("/home/throne/test_") + (char('0' +i)) + string("_send.txt");
         	string file_receive = string("/home/throne/test_") + (char('0' +i)) + string("_receive.txt");
         	path_ofs_send[i].clear();
         	path_ofs_send[i].open(file_send.c_str(),ios::in | ios::out | ios::binary |ios::app);
         	path_ofs_receive[i].clear();
         	path_ofs_receive[i].open(file_receive.c_str(),ios::in | ios::out | ios::binary | ios::app);




        	}

        	//test 1
   //     	mocap_vrpn_sub_1 = nh.subscribe(string("/vrpn_client_node/") + string("MiniQuad_") + test_num + string("/pose"), 1, &TestOpt::optitrack_pose_callback, this);
   //     	mocap_vrpn_STANDERD_sub = nh.subscribe(string("/vrpn_client_node/") + string("MiniQuad_") + STANDERD_num + string("/pose"), 1, &TestOpt::optitrack_pose_standerd_callback, this);

        	//test 2
        	//test transform time


        	pose_opti_enu = Affine3d::Identity();
        	pose_opti_enu.linear()<<1,0,0,0,0,-1,0,1,0;

        	Matrix3d pose_init_uav_enu;
        	pose_init_uav_enu<<0,-1,0,1,0,0,0,0,1;

        	pose_uav_rb.linear() = pose_opti_enu.linear().inverse() * pose_init_uav_enu;
        	pose_uav_rb.translation() = Vector3d::Zero();
        }

        ~TestOpt(){}
        void keyboardLoop();
        void getkey_cb(const boost::shared_ptr<bool const>& msg){}
        void getPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){}
        void stopRobot()
        {

        	path_ofs_1.close();
        	path_ofs_2.close();

        	for(int i = 0;i<uav_num_now;i++)
        	{
        	path_ofs_send[i].close();
        	path_ofs_receive[i].close();
        	}
        }
};

Vector3d to_euler2(Eigen::Matrix<double,3,3> data){
	Vector3d euler;
	euler(1) = asinf(- (float)data(2,0));

	if (fabsf((float)euler(1) -1.5708f) < 1.0e-3f) {
		euler(0) = 0.0f;
		euler(2) = atan2f((float)data(1,2) - (float)data(0,1), (float)data(0,2) + (float)data(1,1)) + euler(0);

	} else if (fabsf((float)euler(1) + 1.5708f) < 1.0e-3f) {
		euler(0) = 0.0f;
		euler(2) =atan2f((float)data(1,2) - (float)data(0,1), (float)data(0,2) + (float)data(1,1)) - euler(0);

	} else {
		euler(0) = atan2f((float)data(2,1), (float)data(2,2));
		euler(2) = atan2f((float)data(1,0), (float)data(0,0));
	}

	return euler;
}

void TestOpt::send_uav_pose(int uav_num)
{
	//ROS_INFO("Send_OK");
		static int count = 11;
		static int count2 = 0;

		static int num2 = 0;
		num2++;
		 count ++;
		 count2 ++;

		 if(count>5001)
		 {
			 count = 3;
		 }

		 if(count2>6211)
		{
		    count2 = 15;
	    }
//		 if (num2%10 == 0)
//		 {
//			 count += 1;
//		 }
//		 if (num2%30 == 0)
//		{
//			 count2 += 1;
//		}
		opti_pose_test_relay.translation().x()  = count ;
		opti_pose_test_relay.translation().y()  = count2 ;

		geometry_msgs::PoseStamped pose_msg;
		pose_msg.header.stamp = ros::Time::now();
		double time_now = ros::Time::now().toSec() - timebias;

		tf::poseEigenToMsg(opti_pose_test_relay,pose_msg.pose);



		for(int i =0;i<uav_num_now;i++)
		{
		position_pub[i].publish(pose_msg);
		//
		path_ofs_send[i]<<pose_msg.pose.position.x<<'\t' <<pose_msg.pose.position.y <<'\t'<<pose_msg.pose.position.z<<'\t'<<time_now<<endl;
		}
}
void TestOpt::mavros_pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	tf::poseMsgToEigen(msg->pose,mavros_pose[0]);
	//ROS_INFO("Receive_OK_num0");

	 double time_now = ros::Time::now().toSec() - timebias;
	//
	path_ofs_receive[0]<<msg->pose.position.x<<'\t' <<msg->pose.position.y <<'\t'<<msg->pose.position.z<<'\t'<<time_now<<endl;
}

void TestOpt::mavros_pose_callback2(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	tf::poseMsgToEigen(msg->pose,mavros_pose[1]);
	//ROS_INFO("Receive_OK_num1");

	 double time_now = ros::Time::now().toSec() - timebias;
	//
	path_ofs_receive[1]<<msg->pose.position.x<<'\t' <<msg->pose.position.y <<'\t'<<msg->pose.position.z<<'\t'<<time_now<<endl;
}

void TestOpt::mavros_pose_callback3(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	tf::poseMsgToEigen(msg->pose,mavros_pose[2]);
	//ROS_INFO("Receive_OK_num2");

	 double time_now = ros::Time::now().toSec() - timebias;
	//
	 ROS_INFO("X=%f",msg->pose.position.x);
	path_ofs_receive[2]<<msg->pose.position.x<<'\t' <<msg->pose.position.y <<'\t'<<msg->pose.position.z<<'\t'<<time_now<<endl;
}
//
void TestOpt::optitrack_pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	//last_optitrack_timestamp = ros::Time::now().toSec();
	Affine3d pose_rb_opti;
	tf::poseMsgToEigen(msg->pose,pose_rb_opti);


	opti_pose = pose_opti_enu * pose_rb_opti * pose_uav_rb;

	Vector3d euler2;
	euler2 = to_euler2(opti_pose.linear());

	Vector3d xyz;
	xyz.x() = opti_pose.translation().x();
	xyz.y() = opti_pose.translation().y();
	xyz.z() = opti_pose.translation().z();


	 double time_now = ros::Time::now().toSec() - timebias;


	//path_ofs_1<< xyz.x()<<'\t' <<xyz.y() <<'\t'<<xyz.z()<<'\t'<<euler2.x()<<'\t'<<euler2.y()<<'\t'<<euler2.z()<<'\t'<<time_now<<'\n';
}
void TestOpt::optitrack_pose_standerd_callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	//last_optitrack_timestamp = ros::Time::now().toSec();
	Affine3d pose_rb_opti;
	tf::poseMsgToEigen(msg->pose,pose_rb_opti);



	opti_pose_standerd = pose_opti_enu * pose_rb_opti * pose_uav_rb;
	//save
	Vector3d euler2;
	euler2 = to_euler2(opti_pose_standerd.linear());

	Vector3d xyz;
	xyz.x() = opti_pose_standerd.translation().x();
	xyz.y() = opti_pose_standerd.translation().y();
	xyz.z() = opti_pose_standerd.translation().z();

	 double time_now = ros::Time::now().toSec() - timebias;

//	path_ofs_2<< xyz.x()<<'\t' <<xyz.y() <<'\t'<<xyz.z()<<'\t'<<euler2.x()<<'\t'<<euler2.y()<<'\t'<<euler2.z()<<'\t'<<time_now<<'\n';


}

//
TestOpt* tbk;
int kfd = 0;

//ofstream path("/home/throne/test_err.txt",ios::in | ios::out | ios::binary |ios::app);

int main(int argc, char** argv)
{
    ros::init(argc,argv,"test_opt", ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);

    int uav_num = 3;
    double MAIN_CYCLE_RATE = 120.0;
    if(!ros::param::get("~uav_num", uav_num))
     {
       	ROS_ERROR("must set uav number _uav_num:=[num]");
       	return 1;
     }
    TestOpt tbk(uav_num);
    if(!ros::param::get("~fre", MAIN_CYCLE_RATE))
      {
        	ROS_ERROR("must set fre _fre:=[num]");
        	return 1;
      }

    ROS_INFO("uav_num = %d, fre = %.6lf\n",uav_num,MAIN_CYCLE_RATE);
    sleep(5);


    double MAIN_CYCLE_INTERVAL = 1.0 / MAIN_CYCLE_RATE;
    ros::Rate main_rate(MAIN_CYCLE_RATE);

    while(ros::ok())
    {
    ros::spinOnce();
    main_rate.sleep();
    tbk.keyboardLoop();
    fflush(stdin);
    }
    ros::spin();
    tbk.stopRobot();

  //  tcsetattr(kfd, TCSANOW, &cooked);
   // path.close();
    return(0);
}


void TestOpt::keyboardLoop()
{

    //tcsetattr(kfd, TCSANOW, &raw);
    static int co = 0;
    {
    	co++;
//    	double time_n =  ros::Time::now().toSec() - timebias;

//    	Vector3d errdis;
//    	Vector3d errangle;

//    	errdis = opti_pose_standerd.translation() - opti_pose.translation();
//    	errangle = to_euler2(opti_pose_standerd.linear()) - to_euler2(opti_pose.linear());

//    	if(co%5000)
//    	{

    	//test1
//    	if(co>20)
//    	{
//    	path<< co<<'\t'<<errdis.x()<<'\t' <<errdis.y() <<'\t'<<errdis.z()<<'\t'<<errangle.x()<<'\t'<<errangle.y()<<'\t'<<errangle.z()<<'\t'<<time_n<<'\n';
//    	}
//
    	//test2


    	send_uav_pose(0);
//    	for (int i =0;i<uav_num_now;i++)
//    	{
//    	send_uav_pose(i);
//    	}

    }

}
